//--------------------------------------------------------------------------------
// This file is a portion of the PracticeEngine Rendering Engine.  It is distributed
// under the MIT License, available in the root of this distribution and 
// at the following URL:
//
// http://www.opensource.org/licenses/mit-license.php
//
// Copyright (c) Ferdinand Li 
//--------------------------------------------------------------------------------
#include "PEMath.h"
#include <math.h>
#include <memory>
using namespace PracEng;
#pragma warning(push)
#pragma warning(disable:4305)

PEVector3::PEVector3() :x(0.0f), y(0.0f), z(0.0f)
{

}



PEVector3::PEVector3(float X, float Y, float Z) :x(X), y(Y), z(Z)
{

}



PEVector3::PEVector3(const PEVector3& Vector) :x(Vector.x), y(Vector.y), z(Vector.z)
{

}

PEVector3::PEVector3(float Val) : x(Val), y(Val), z(Val)
{

}

PEVector3 PEVector3::Cross(const PEVector3& RefVector) const
{
	PEVector3 Val;
	Val.x = y*RefVector.z - z*RefVector.y;
	Val.y = z*RefVector.x - x*RefVector.z;
	Val.z = x*RefVector.y - y*RefVector.x;

	return Val;
}

PEVector3 PEVector3::Cross(const PEVector3& A, const PEVector3& B)
{
	return A.Cross(B);
}

float PEVector3::Dot(const PEVector3& RefVector) const
{
	float Val = .0f;
	Val = x* RefVector.x;
	Val += y*RefVector.y;
	Val += z*RefVector.z;

	return Val;
}

float PEVector3::Dot(const PEVector3& A, const PEVector3& B)
{
	return A.Dot(B);
}

void PEVector3::Normalize()
{
	float Len = Length();
	if (Len == 0.0f) Len = (1.e-4);

	float fInvLen = (1.0f / Len);
	x *= fInvLen;
	y *= fInvLen;
	z *= fInvLen;
}

PEVector3 PEVector3::Normalize(const PEVector3& A)
{
	PEVector3 Val = A;
	Val.Normalize();
	return Val;
}

PEVector3& PEVector3::operator=(const PEVector3& Vector)
{
	x = Vector.x;
	y = Vector.y;
	z = Vector.z;

	return (*this);
}

bool PEVector3::operator==(const PEVector3& Vector) const
{
	return ((x - Vector.x)*(x - Vector.x) < 0.0001f) && ((y - Vector.y)*(y - Vector.y) < 0.0001f) && ((z - Vector.z)*(z - Vector.z) < 0.0001f);
}

PEVector3& PEVector3::operator=(const PEVector4& Vector)
{
	x = Vector.x;
	y = Vector.y;
	z = Vector.z;

	return (*this);
}

bool PEVector3::operator!=(const PEVector3& Vector) const
{
	return !(*this == Vector);
}

PEVector3 PEVector3::operator+(const PEVector3& Vector) const
{
	PEVector3 sum;
	sum.x = x + Vector.x;
	sum.y = y + Vector.y;
	sum.z = z + Vector.z;

	return sum;
}

PEVector3 PEVector3::operator-(const PEVector3& Vector) const
{
	PEVector3 diff;
	diff.x = x - Vector.x;
	diff.y = y - Vector.y;
	diff.z = z - Vector.z;

	return diff;
}

PEVector3 PEVector3::operator*(const PEVector3& Vector) const
{
	PEVector3 product;

	product.x = x * Vector.x;
	product.y = y * Vector.y;
	product.z = z * Vector.z;

	return product;
}

PEVector3 PEVector3::operator*(float fScalar) const
{
	PEVector3 product;
	product.x = x*fScalar;
	product.y = y*fScalar;
	product.z = z*fScalar;

	return product;
}

PEVector3 PEVector3::operator/(const PEVector3& Vector) const
{
	PEVector3 Val;
	Val.x = x / Vector.x;
	Val.y = y / Vector.y;
	Val.z = z / Vector.z;

	return Val;
}

PEVector3 PEVector3::operator/(float fScalar) const
{
	PEVector3 Val;
	Val.x = x*fScalar;
	Val.y = y*fScalar;
	Val.z = z*fScalar;

	return Val;
}

PEVector3 PEVector3::operator-() const
{
	PEVector3 Val;
	Val.x = -x;
	Val.y = -y;
	Val.z = -z;

	return Val;
}

float PEVector3::LengthSquared() const
{
	return x*x + y*y + z*z;
}

float PEVector3::Length() const
{
	return sqrt(LengthSquared());
}

PEVector3& PEVector3::operator+=(const PEVector3& Vector)
{
	x += Vector.x;
	y += Vector.y;
	z += Vector.z;
	return (*this);
}

PEVector3& PEVector3::operator-=(const PEVector3& Vector)
{
	x -= Vector.x;
	y -= Vector.y;
	z -= Vector.z;
	return (*this);
}

PEVector3& PEVector3::operator*=(const PEVector3& Vector)
{
	x *= Vector.x;
	y *= Vector.y;
	z *= Vector.z;

	return (*this);
}

PEVector3& PEVector3::operator*=(float fScalar)
{
	x *= fScalar;
	y *= fScalar;
	z *= fScalar;

	return (*this);
}

PEVector3& PEVector3::operator/=(const PEVector3& Vector)
{
	x /= Vector.x;
	y /= Vector.y;
	z /= Vector.z;

	return (*this);
}

PEVector3& PEVector3::operator/=(float fScalar)
{
	x /= fScalar;
	y /= fScalar;
	z /= fScalar;

	return (*this);
}

float PEVector3::operator[](int iPos) const
{
	if (iPos == 0) return x;
	if (iPos == 1) return y;
	return z;
}

float& PEVector3::operator[](int iPos)
{
	if (iPos == 0) return x;
	if (iPos == 1) return y;
	return z;
}

float PEVector3::CalcAngleBetween(const PEVector3& A, const PEVector3& B)
{
	float LengthQ = A.Length()*B.Length();

	if (LengthQ < 0.0001f)LengthQ = 0.0001f;			// to prevent division by zero

	float f = A.Dot(B) / LengthQ;

	if (f > 1.0f)f = 1.0f;												// acos need input in the range [-1..1]
	else if (f < -1.0f)f = -1.0f;							//

	float fRet = (float)acos(f);								// cosf is not avaiable on every plattform

	return fRet;
}

PEVector4::PEVector4() :x(.0f), y(.0f), z(.0f), w(.0f)
{

}

PEVector4::PEVector4(float X, float Y, float Z, float W) : x(X), y(Y), z(Z), w(W)
{

}

PEVector4::PEVector4(const PEVector4& ref) : x(ref.x), y(ref.y), z(ref.z), w(ref.w)
{

}

PEVector4::PEVector4(const PEVector3& ref, float W /*= 1.0f*/) : x(ref.x), y(ref.y), z(ref.z), w(W)
{

}

float PEVector4::Dot(PEVector4& vector)
{
	return x*vector.x + y*vector.y + z*vector.z + w*vector.w;
}

PEVector4& PEVector4::operator=(const PEVector4& Vector)
{
	x = Vector.x;
	y = Vector.y;
	z = Vector.z;
	w = Vector.w;

	return (*this);
}

float PEVector4::operator[](int iPos) const
{
	if (iPos == 0) return x;
	if (iPos == 1) return y;
	if (iPos == 2) return z;
	return w;
}

float& PEVector4::operator[](int iPos)
{
	if (iPos == 0) return x;
	if (iPos == 1) return y;
	if (iPos == 2) return z;
	return w;
}

bool PEVector4::operator==(const PEVector4& Vector) const
{
	return ((x - Vector.x)*(x - Vector.x) < 0.0001f) && ((y - Vector.y)*(y - Vector.y) < 0.0001f) && ((z - Vector.z)*(z - Vector.z) < 0.0001f) && ((w - Vector.w)*(w - Vector.w) < 0.0001f);
}

bool PEVector4::operator!=(const PEVector4& Vector) const
{
	return !(*this == Vector);
}

PEVector4 PEVector4::operator+(const PEVector4& Vector) const
{
	PEVector4 sum;

	sum.x = x + Vector.x;
	sum.y = y + Vector.y;
	sum.z = z + Vector.z;
	sum.w = w + Vector.w;

	return sum;
}

PEVector4 PEVector4::operator-(const PEVector4& Vector) const
{
	PEVector4 diff;
	diff.x = x - Vector.x;
	diff.y = y - Vector.y;
	diff.z = z - Vector.z;
	diff.w = w - Vector.w;

	return diff;
}

PEVector4 PEVector4::operator*(float fScalar) const
{
	PEVector4 product;
	product.x = x*fScalar;
	product.y = y*fScalar;
	product.z = z*fScalar;
	product.w = w*fScalar;

	return product;
}

PEVector4 PEVector4::operator*(const PEVector4& Vector) const
{
	PEVector4 product;
	product.x = x*Vector.x;
	product.y = y*Vector.y;
	product.z = z*Vector.z;
	product.w = w*Vector.w;

	return product;
}

PEVector4 PEVector4::operator/(float fScalar) const
{
	PEVector4 val;
	float Inv = 1.0f / fScalar;
	val.x = x * Inv;
	val.y = y * Inv;
	val.z = z * Inv;
	val.w = w * Inv;

	return val;
}

PEVector4 PEVector4::operator/(const PEVector4& Vector) const
{
	PEVector4 val;

	val.x = x / Vector.x;
	val.y = y / Vector.y;
	val.z = z / Vector.z;
	val.w = w / Vector.w;

	return val;
}

PEVector4& PEVector4::operator+=(const PEVector4& Vector)
{
	x += Vector.x;
	y += Vector.y;
	z += Vector.z;
	w += Vector.w;

	return (*this);
}

PEVector4& PEVector4::operator-=(const PEVector4& Vector)
{
	x -= Vector.x;
	y -= Vector.y;
	z -= Vector.z;
	w -= Vector.w;

	return (*this);
}

PEVector4& PEVector4::operator*=(float fScalar)
{
	x *= fScalar;
	y *= fScalar;
	z *= fScalar;
	w *= fScalar;

	return (*this);
}

PEVector4& PEVector4::operator*=(const PEVector4& Vector)
{
	x *= Vector.x;
	y *= Vector.y;
	z *= Vector.z;
	w *= Vector.w;

	return (*this);
}

PEVector4& PEVector4::operator/=(float fScalar)
{
	float fInvScalar = 1.0f / fScalar;

	x *= fInvScalar;
	y *= fInvScalar;
	z *= fInvScalar;
	w *= fInvScalar;

	return (*this);
}

PEVector4& PEVector4::operator/=(const PEVector4& Vector)
{
	x /= Vector.x;
	y /= Vector.y;
	z /= Vector.z;
	w /= Vector.w;

	return (*this);
}

PEVector3 PracEng::PEVector4::Vec3()
{
	return PEVector3(x, y, z);
}

PEMatrix3x3::PEMatrix3x3()
{
	memset(m, 0, 9 * sizeof(float));
}

PEMatrix3x3::PEMatrix3x3(const PEMatrix3x3& M)
{
	memcpy(m, (void*)&M, 9 * sizeof(float));
}

PEMatrix3x3::PEMatrix3x3(const PEVector3& Row1, const PEVector3& Row2, const PEVector3& Row3)
{
	_11 = Row1.x; _12 = Row1.y; _13 = Row1.z;
	_21 = Row2.x; _22 = Row2.y; _23 = Row2.z;
	_31 = Row3.x; _32 = Row3.y; _33 = Row2.z;
}

void PEMatrix3x3::Rotation(PEVector3& Rot)
{
	PEMatrix3x3 mRot1;
	PEMatrix3x3 mRot2;

	mRot1.RotationZ(Rot.z);
	mRot2.RotationX(Rot.x);
	mRot1 *= mRot2;
	*this = mRot1;
}

PEMatrix3x3 PEMatrix3x3::GetIdentityMatrix()
{
	PEMatrix3x3 mat;
	mat._11 = mat._22 = mat._33 = 1.0f;
	return mat;
}

PEMatrix3x3& PEMatrix3x3::operator=(const PEMatrix3x3& M)
{
	memcpy(m, M.m, 9 * sizeof(float));
	return *this;
}

float PEMatrix3x3::operator()(int iRow, int iCol) const
{
	return m[iRow][iCol];
}

float& PEMatrix3x3::operator()(int iRow, int iCol)
{
	return m[iRow][iCol];
}


bool PEMatrix3x3::operator==(const PEMatrix3x3& Matrix) const
{
	return(memcmp(m, Matrix.m, 3 * 3 * sizeof(float)) == 0);
}

bool PEMatrix3x3::operator!=(const PEMatrix3x3& Matrix) const
{
	return (memcmp(m, Matrix.m, 3 * 3 * sizeof(float)) != 0);
}

PEMatrix3x3 PEMatrix3x3::operator+(const PEMatrix3x3& Matrix) const
{
	PEMatrix3x3 sum;
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			sum.m[i][j] = m[i][j] + Matrix.m[i][j];
		}
	}
	return sum;
}

PEMatrix3x3 PEMatrix3x3::operator-(const PEMatrix3x3& Matrix) const
{
	PEMatrix3x3 diff;

	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			diff.m[i][j] = m[i][j] - Matrix.m[i][j];
		}
	}

	return diff;
}

PEMatrix3x3 PEMatrix3x3::operator*(const PEMatrix3x3& Matrix) const
{
	PEMatrix3x3 multi;

	multi._11 = _11 * Matrix._11 + _12 * Matrix._21 + _13 * Matrix._31;
	multi._12 = _11 * Matrix._12 + _12 * Matrix._22 + _13 * Matrix._32;
	multi._13 = _11 * Matrix._13 + _12 * Matrix._23 + _13 * Matrix._33;

	multi._21 = _21 * Matrix._11 + _22 * Matrix._21 + _23 * Matrix._31;
	multi._22 = _21 * Matrix._12 + _22 * Matrix._22 + _23 * Matrix._32;
	multi._23 = _21 * Matrix._13 + _22 * Matrix._23 + _23 * Matrix._33;
	
	multi._31 = _31 * Matrix._11 + _32 * Matrix._21 + _33 * Matrix._31;
	multi._32 = _31 * Matrix._12 + _32 * Matrix._22 + _33 * Matrix._32;
	multi._33 = _31 * Matrix._13 + _32 * Matrix._23 + _33 * Matrix._33;

	return multi;

}

PEMatrix3x3 PEMatrix3x3::operator*(float fScalar) const
{
	PEMatrix3x3 val;

	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			val.m[i][j] = m[i][j] * fScalar;
		}
	}

	return val;
}

PEVector3 PEMatrix3x3::operator*(const PEVector3& Vector) const
{
	PEVector3 Product;

	Product.x = _11*Vector.x + _12*Vector.y + _13*Vector.z;
	Product.y = _21*Vector.x + _22*Vector.y + _23*Vector.z;
	Product.z = _31*Vector.x + _32*Vector.y + _33*Vector.z;

	return Product;
}

PEMatrix3x3 PEMatrix3x3::operator/(float fScalar) const
{
	PEMatrix3x3 val;
	float InvScalar = 1.0f / fScalar;
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			val.m[i][j] = m[i][j] * InvScalar;
		}
	}

	return val;
}

PEMatrix3x3 PEMatrix3x3::operator-() const
{
	PEMatrix3x3 val;
	
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			val.m[i][j] = -m[i][j];
		}
	}

	return val;
}

PEMatrix3x3& PEMatrix3x3::operator+=(const PEMatrix3x3& Matrix)
{
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			m[i][j] += Matrix.m[i][j];
		}
	}

	return (*this);
}

PEMatrix3x3& PEMatrix3x3::operator-=(const PEMatrix3x3& Matrix)
{
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			m[i][j] -= Matrix.m[i][j];
		}
	}

	return (*this);
}

PEMatrix3x3& PEMatrix3x3::operator*=(const PEMatrix3x3& Matrix)
{
	_11 = _11 * Matrix._11 + _12 * Matrix._21 + _13 * Matrix._31;
	_12 = _11 * Matrix._12 + _12 * Matrix._22 + _13 * Matrix._32;
	_13 = _11 * Matrix._13 + _12 * Matrix._23 + _13 * Matrix._33;

	_21 = _21 * Matrix._11 + _22 * Matrix._21 + _23 * Matrix._31;
	_22 = _21 * Matrix._12 + _22 * Matrix._22 + _23 * Matrix._32;
	_23 = _21 * Matrix._13 + _22 * Matrix._23 + _23 * Matrix._33;

	_31 = _31 * Matrix._11 + _32 * Matrix._21 + _33 * Matrix._31;
	_32 = _31 * Matrix._12 + _32 * Matrix._22 + _33 * Matrix._32;
	_33 = _31 * Matrix._13 + _32 * Matrix._23 + _33 * Matrix._33;

	return (*this);
}

PEMatrix3x3& PEMatrix3x3::operator*=(float fScalar)
{
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			m[i][j] *= fScalar;
		}
	}
	return (*this);
}

PEMatrix3x3& PEMatrix3x3::operator/=(float fScalar)
{
	float InvScalar = 1.0f / fScalar;
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			m[i][j] *= InvScalar;
		}
	}
	return (*this);
}

void PEMatrix3x3::RotationX(float fRadians)
{
	float fSin = sinf(fRadians);
	float fCos = cosf(fRadians);

	_11 = 1.0f; _12 = 0.0f; _13 = 0.0f;
	_21 = 0.0f; _22 = fCos; _23 = fSin;
	_31 = 0.0f; _32 = -fSin; _33 = fCos;
}

void PEMatrix3x3::RotationY(float fRadians)
{
	float fSin = sinf(fRadians);
	float fCos = cosf(fRadians);

	_11 = fCos; _12 = 0.0f; _13 = -fSin;
	_21 = 0.0f; _22 = 1.0f; _23 = 0.0f;
	_31 = fSin; _32 = 0.0f; _33 = fCos;
}

void PEMatrix3x3::RotationZ(float fRadians)
{
	float fSin = sinf(fRadians);
	float fCos = cosf(fRadians);

	_11 = fCos; _12 = fSin; _13 = 0.0f;
	_21 = -fSin; _22 = fCos; _23 = 0.0f;
	_31 = 0.0f; _32 = 0.0f; _33 = 1.0f;
}


PEMatrix::PEMatrix()
{
	memset(m, 0, 16 * sizeof(float));
}

PEMatrix::PEMatrix(const PEVector4& Row1, const PEVector4& Row2, const PEVector4& Row3, const PEVector4& Row4)
	:_11(Row1.x), _12(Row1.y), _13(Row1.z), _14(Row1.w)
	, _21(Row2.x), _22(Row2.y), _23(Row2.z), _24(Row2.w)
	, _31(Row3.x), _32(Row3.y), _33(Row3.z), _34(Row3.w)
	, _41(Row4.x), _42(Row4.y), _43(Row4.z), _44(Row4.w)
{

}

PEMatrix& PEMatrix::operator=(const PEMatrix& M)
{
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			m[i][j] = M.m[i][j];
		}
	}

	return (*this);
}

PEMatrix PEMatrix::operator-() const
{
	PEMatrix Val;

	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			Val.m[i][j] = -m[i][j];
		}
	}

	return Val;
}

PEMatrix& PEMatrix::operator+=(const PEMatrix& M)
{
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			m[i][j] += M.m[i][j];
		}
	}

	return (*this);
}

PEMatrix PEMatrix::operator+(const PEMatrix& Matrix) const
{
	PEMatrix Val;
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			Val.m[i][j] = m[i][j] + Matrix.m[i][j];
		}
	}

	return Val;
}

PEMatrix PEMatrix::operator-(const PEMatrix& Matrix) const
{
	PEMatrix Val;
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			Val.m[i][j] = m[i][j] - Matrix.m[i][j];
		}
	}

	return Val;
}

PEMatrix& PEMatrix::operator-=(const PEMatrix& M)
{
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			m[i][j] -= M.m[i][j];
		}
	}

	return (*this);
}

PEMatrix& PEMatrix::operator*=(const PEMatrix& M)
{
	_11 = _11 * M._11 + _12 * M._21 + _13 * M._31 + _14 * M._41;
	_12 = _11 * M._12 + _12 * M._22 + _13 * M._32 + _14 * M._42;
	_13 = _11 * M._13 + _12 * M._23 + _13 * M._33 + _14 * M._43;
	_14 = _11 * M._14 + _12 * M._24 + _13 * M._34 + _14 * M._44;

	_21 = _21 * M._11 + _22 * M._21 + _23 * M._31 + _24 * M._41;
	_22 = _21 * M._12 + _22 * M._22 + _23 * M._32 + _24 * M._42;
	_23 = _21 * M._13 + _22 * M._23 + _23 * M._33 + _24 * M._43;
	_24 = _21 * M._14 + _22 * M._24 + _23 * M._34 + _24 * M._44;

	_31 = _31 * M._11 + _32 * M._21 + _33 * M._31 + _34 * M._41;
	_32 = _31 * M._12 + _32 * M._22 + _33 * M._32 + _34 * M._42;
	_33 = _31 * M._13 + _32 * M._23 + _33 * M._33 + _34 * M._43;
	_34 = _31 * M._14 + _32 * M._24 + _33 * M._34 + _34 * M._44;

	_41 = _41 * M._11 + _42 * M._21 + _43 * M._31 + _44 * M._41;
	_42 = _41 * M._12 + _42 * M._22 + _43 * M._32 + _44 * M._42;
	_43 = _41 * M._13 + _42 * M._23 + _43 * M._33 + _44 * M._43;
	_44 = _41 * M._14 + _42 * M._24 + _43 * M._34 + _44 * M._44;

	return (*this);
}

PEMatrix& PEMatrix::operator*=(float fScalar)
{
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			m[i][j] *= fScalar;
		}
	}

	return (*this);
}

PEMatrix& PEMatrix::operator/=(float fScalar)
{
	float InvScalar = 1.0f / fScalar;
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			m[i][j] *= InvScalar;
		}
	}

	return (*this);
}

PEMatrix PEMatrix::operator/(float fScalar) const
{
	PEMatrix Div;
	float InvScalar = 1.0f / fScalar;
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			Div.m[i][j] = m[i][j] * InvScalar;
		}
	}
	return Div;
}

PEVector4 PEMatrix::operator*(const PEVector4& V) const
{
	PEVector4 Product;

	/*Product.x = _11*V.x + _12*V.y + _13*V.z + _14*V.w;
	Product.y = _21*V.x + _22*V.y + _23*V.z + _24*V.w;
	Product.z = _31*V.x + _32*V.y + _33*V.z + _34*V.w;
	Product.w = _41*V.x + _42*V.y + _43*V.z + _44*V.w;*/
	
	Product.x = V.x * _11 + V.y * _21 + V.z * _31 + V.w*_41;
	Product.y = V.x * _12 + V.y * _22 + V.z * _32 + V.w*_42;
	Product.z = V.x * _13 + V.y * _23 + V.z * _33 + V.w*_43;
	Product.w = V.x * _14 + V.y * _24 + V.z * _34 + V.w*_44;

	return Product;
}

PEMatrix PEMatrix::operator*(const PEMatrix& M) const
{
	PEMatrix Product;
	Product._11 = _11 * M._11 + _12 * M._21 + _13 * M._31 + _14 * M._41;
	Product._12 = _11 * M._12 + _12 * M._22 + _13 * M._32 + _14 * M._42;
	Product._13 = _11 * M._13 + _12 * M._23 + _13 * M._33 + _14 * M._43;
	Product._14 = _11 * M._14 + _12 * M._24 + _13 * M._34 + _14 * M._44;

	Product._21 = _21 * M._11 + _22 * M._21 + _23 * M._31 + _24 * M._41;
	Product._22 = _21 * M._12 + _22 * M._22 + _23 * M._32 + _24 * M._42;
	Product._23 = _21 * M._13 + _22 * M._23 + _23 * M._33 + _24 * M._43;
	Product._24 = _21 * M._14 + _22 * M._24 + _23 * M._34 + _24 * M._44;

	Product._31 = _31 * M._11 + _32 * M._21 + _33 * M._31 + _34 * M._41;
	Product._32 = _31 * M._12 + _32 * M._22 + _33 * M._32 + _34 * M._42;
	Product._33 = _31 * M._13 + _32 * M._23 + _33 * M._33 + _34 * M._43;
	Product._34 = _31 * M._14 + _32 * M._24 + _33 * M._34 + _34 * M._44;

	Product._41 = _41 * M._11 + _42 * M._21 + _43 * M._31 + _44 * M._41;
	Product._42 = _41 * M._12 + _42 * M._22 + _43 * M._32 + _44 * M._42;
	Product._43 = _41 * M._13 + _42 * M._23 + _43 * M._33 + _44 * M._43;
	Product._44 = _41 * M._14 + _42 * M._24 + _43 * M._34 + _44 * M._44;

	return Product;
}

PEMatrix PEMatrix::operator*(float fScalar) const
{
	PEMatrix Product;
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			Product.m[i][j] = m[i][j] * fScalar;
		}
	}
	return Product;
}

PEVector3 PEMatrix::operator*(PEVector3 Vec) const
{
	PEVector3 Product;
	PEVector4 V(Vec);
	Product.x = V.x * _11 + V.y * _21 + V.z * _31 + V.w*_41;
	Product.y = V.x * _12 + V.y * _22 + V.z * _32 + V.w*_42;
	Product.z = V.x * _13 + V.y * _23 + V.z * _33 + V.w*_43;

	return Product;
}

PEVector3 PEMatrix::GetBasisX() const
{
	PEVector3 BasisX;

	BasisX.x = _11;
	BasisX.y = _12;
	BasisX.z = _13;

	return BasisX;
}

PEVector3 PEMatrix::GetBasisY() const
{
	PEVector3 BasisY;

	BasisY.x = _21;
	BasisY.y = _22;
	BasisY.z = _23;

	return BasisY;
}

PEVector3 PEMatrix::GetBasisZ() const
{
	PEVector3 BasisZ;

	BasisZ.x = _31;
	BasisZ.y = _32;
	BasisZ.z = _33;

	return BasisZ;
}

PEVector3 PEMatrix::GetTranslation() const
{
	PEVector3 Trans;

	Trans.x = _41;
	Trans.y = _42;
	Trans.z = _43;

	return Trans;
}

PEMatrix3x3 PEMatrix::GetRotation() const
{
	return PEMatrix3x3(GetBasisX(), GetBasisY(), GetBasisZ());
}

void PEMatrix::SetTranslation(const PEVector3& Trans)
{
	_41 = Trans.x; _42 = Trans.y; _43 = Trans.z;
}

void PEMatrix::SetTranslation(const float& x, const float& y, const float& z)
{
	_41 = x; _42 = y; _43 = z;
}

PEMatrix PEMatrix::MakeLookAtLHMatrix(PEVector3* pEyePt, PEVector3* pLookatPt, PEVector3* pLookUp)
{
	PEVector3 Up = pLookUp == NULL ? PEVector3(0, 1, 0) : *pLookUp;
	PEVector3 Toward(*pLookatPt - *pEyePt);
	Toward.Normalize();
	PEVector3 Right = PEVector3::Cross(Up, Toward);
	Right.Normalize();
	Up = PEVector3::Cross(Toward, Right);

	PEVector3 NegEyePos = *pEyePt * -1.0f;

	float D0 = Right.Dot(NegEyePos);
	float D1 = Up.Dot(NegEyePos);
	float D2 = Toward.Dot(NegEyePos);

	PEVector4 row1(Right.x, Up.x, Toward.x, .0f);
	PEVector4 row2(Right.y, Up.y, Toward.y, .0f);
	PEVector4 row3(Right.z, Up.z, Toward.z, .0f);
	PEVector4 row4(D0, D1, D2, 1.0f);
	return PEMatrix(row1, row2, row3, row4);
}

PEMatrix PEMatrix::MakePerspectiveMatrix(float fFOV, float fAspect, float fNearPlane, float fFarPlane)
{
	float TanHalfFOV = 1.0f / tan(fFOV * 0.5f);
	
	PEVector4 row1(TanHalfFOV * 1.0f / fAspect, .0f,				.0f,														.0f);
	PEVector4 row2(.0f,							TanHalfFOV,			.0f,														.0f);
	PEVector4 row3(.0f,							.0f,				fFarPlane / (fFarPlane - fNearPlane),						1.0f);
	PEVector4 row4(.0f,							.0f,				-fNearPlane*fFarPlane / (fFarPlane - fNearPlane),			.0f);

	return PEMatrix(row1, row2, row3, row4);
}

PEMatrix PEMatrix::GetIdentityMatrix()
{
	PEMatrix mat;
	mat._11 = mat._22 = mat._33 = mat._44 = 1.0f;
	return mat;
}

PEMatrix PEMatrix::MakeInverse(const PEMatrix& M)
{
	PEMatrix MT = PEMatrix::MakeTranspose(M);

	PEVector4 V0[4], V1[4];
	V0[0] = PEVector4( MT._31, MT._31, MT._32, MT._32 );
	V1[0] = PEVector4( MT._43, MT._44, MT._43, MT._44 );
	V0[1] = PEVector4( MT._11, MT._11, MT._12, MT._12 );
	V1[1] = PEVector4( MT._23, MT._24, MT._23, MT._24 );
	V0[2] = PEVector4( MT._31, MT._33, MT._11, MT._13 );
	V1[2] = PEVector4( MT._42, MT._44, MT._22, MT._24 );

	PEVector4 D0 = V0[0] * V1[0];
	PEVector4 D1 = V0[1] * V1[1];
	PEVector4 D2 = V0[2] * V1[2];

	V0[0] = PEVector4(MT._33, MT._34, MT._33, MT._34);
	V1[0] = PEVector4(MT._41, MT._41, MT._42, MT._42);
	V0[1] = PEVector4(MT._13, MT._14, MT._13, MT._14);
	V1[1] = PEVector4(MT._21, MT._21, MT._22, MT._22);
	V0[2] = PEVector4(MT._32, MT._34, MT._12, MT._14);
	V1[2] = PEVector4(MT._41, MT._43, MT._21, MT._23);

	D0 = D0 - V0[0] * V1[0];
	D1 = D1 - V0[1] * V1[1];
	D2 = D2 - V0[2] * V1[2];

	V0[0] = PEVector4(MT._22, MT._23, MT._21, MT._22);
	V1[0] = PEVector4(D2.y, D0.y, D0.w, D0.x);
	V0[1] = PEVector4(MT._13, MT._11, MT._12, MT._11);
	V1[1] = PEVector4(D0.w, D2.y, D0.y, D0.z);
	V0[2] = PEVector4(MT._42, MT._43, MT._41, MT._42);
	V1[2] = PEVector4(D2.w, D1.y, D1.w, D1.x);
	V0[3] = PEVector4(MT._33, MT._31, MT._32, MT._31);
	V1[3] = PEVector4(D1.w, D2.w, D1.y, D1.z);

	PEVector4 C0 = V0[0] * V1[0];
	PEVector4 C2 = V0[1] * V1[1];
	PEVector4 C4 = V0[2] * V1[2];
	PEVector4 C6 = V0[3] * V1[3];

	V0[0] = PEVector4(MT._23, MT._24, MT._22, MT._23);
	V1[0] = PEVector4(D0.w, D0.x, D0.y, D2.x);
	V0[1] = PEVector4(MT._14, MT._13, MT._14, MT._12);
	V1[1] = PEVector4(D0.z, D0.y, D2.x, D0.x);
	V0[2] = PEVector4(MT._43, MT._44, MT._42, MT._43);
	V1[2] = PEVector4(D1.w, D1.x, D1.y, D2.z);
	V0[3] = PEVector4(MT._34, MT._33, MT._34, MT._32);
	V1[3] = PEVector4(D1.z, D1.y, D2.z, D1.x);

	C0 = C0 - V0[0] * V1[0];
	C2 = C2 - V0[1] * V1[1];
	C4 = C4 - V0[2] * V1[2];
	C6 = C6 - V0[3] * V1[3];

	V0[0] = PEVector4(MT._24, MT._21, MT._24, MT._21);
	V1[0] = PEVector4(D0.z, D2.y, D2.x, D0.z);
	V0[1] = PEVector4(MT._12, MT._14, MT._11, MT._13);
	V1[1] = PEVector4(D2.y, D0.x, D0.w, D2.x);
	V0[2] = PEVector4(MT._44, MT._41, MT._44, MT._41);
	V1[2] = PEVector4(D1.z, D2.w, D2.z, D1.z);
	V0[3] = PEVector4(MT._32, MT._34, MT._31, MT._33);
	V1[3] = PEVector4(D2.w, D1.x, D1.w, D2.z);

	PEVector4 C1,C3,C5,C7;
	C1 = C0 - V0[0] * V1[0];
	C0 = V0[0] * V1[0] + C0;
	C3 = V0[1] * V1[1] + C2;
	C2 = C2 - V0[1] * V1[1];
	C5 = C4 - V0[2] * V1[2];
	C4 = V0[2] * V1[2] + C4;
	C7 = V0[3] * V1[3] + C6;
	C6 = C6 - V0[3] * V1[3];

	PEVector4 r0(C0.x, C1.y, C0.z, C1.w);
	PEVector4 r1(C2.x, C3.y, C2.z, C3.w);
	PEVector4 r2(C4.x, C5.y, C4.z, C5.w);
	PEVector4 r3(C6.x, C7.y, C6.z, C7.w);
	PEVector4 MT_r0 = PEVector4(MT._11, MT._12, MT._13, MT._14);
	float dotRslt = r0.Dot(MT_r0);
	PEVector4 TheDeterminant(dotRslt, dotRslt, dotRslt, dotRslt);

	PEVector4 Reciprocal = PEVector4(1.0f / TheDeterminant.x, 1.0f / TheDeterminant.y, 1.0f / TheDeterminant.z, 1.0f / TheDeterminant.w);

	r0 = r0 * Reciprocal;
	r1 = r1 * Reciprocal;
	r2 = r2 * Reciprocal;
	r3 = r3 * Reciprocal;
	return PEMatrix(r0, r1, r2, r3);
}

PEMatrix PEMatrix::SetRotationYawPitchRoll(const PEVector4& PitchYawRoll)
{
	PEVector4 Q = PEQuatRotPitchYawFromVector(PitchYawRoll);
	return PEMatrixRotQuat(Q);
}

PEMatrix PEMatrix::MakeTranspose(const PEMatrix& M)
{
	PEMatrix mat;
	for (int row = 0; row < 4; ++row)
	{
		for (int col = 0; col < 4; ++col)
		{
			mat.m[row][col] = M.m[col][row];
		}
	}

	return mat;
}

void PEMatrix::Transpose()
{
	PEMatrix mat;
	memcpy(mat.m, m, 16 * sizeof(float));
	for (int row = 0; row < 4; ++row)
	{
		for (int col = 0; col < 4; ++col)
		{
			m[row][col] = mat.m[col][row];
		}
	}
}

void PEMatrix::Scale(float xScale, float yScale, float zScale)
{
	_11 *= xScale;
	_22 *= yScale;
	_33 *= zScale;
	_44 = 1.0f;
}

void PEMatrix::RotationY(float fRadians)
{
	float fSin = sinf(fRadians);
	float fCos = cosf(fRadians);
	
	_11 = fCos; _12 = 0.0f; _13 = -fSin;
	_21 = 0.0f; _22 = 1.0f; _23 = 0.0f;
	_31 = fSin; _32 = 0.0f; _33 = fCos;
}

PEMatrix PEMatrix::MakeOrthoMatrix(float w, float h, float zn, float zf)
{
	
	float fRange = 1.0f / (zf - zn);
	PEMatrix M;
	M.m[0][0] = 2.0f / w;
	M.m[0][1] = 0.0f;
	M.m[0][2] = 0.0f;
	M.m[0][3] = 0.0f;

	M.m[1][0] = 0.0f;
	M.m[1][1] = 2.0f / h;
	M.m[1][2] = 0.0f;
	M.m[1][3] = 0.0f;

	M.m[2][0] = 0.0f;
	M.m[2][1] = 0.0f;
	M.m[2][2] = fRange;
	M.m[2][3] = 0.0f;

	M.m[3][0] = 0.0f;
	M.m[3][1] = 0.0f;
	M.m[3][2] = -fRange * zn;
	M.m[3][3] = 1.0f;
	return M;
}


#pragma warning(pop)

